clear all
%close all

springN		= 1;
save_plots	= 0;

fprintf('\nStiffness script START\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
log_name = './sine_track_test';

str = sprintf('./spring%d/sys%d.mat',springN,springN);
load(str);

tauc	= sys.tauc;
GR		= sys.GR;

K		= sys.K;
B		= sys.B;
I		= sys.I;
OFFSET	= sys.OFFSET;

A2T = tauc*GR;

t_start = 3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run controllers

f = 12;

t_end	= t_start+3*1/f;

a		= 10;
b		= 0;
w		= 2*pi*f;
k1		= 10000.0;
k2		= 10.0;
ke1		= 20000.0;
ke2		= 300.0;
trq_lim = 15;

ctrl.a			= a;
ctrl.b			= b;
ctrl.k1			= k1;
ctrl.k2			= k2;
ctrl.ke1		= ke1;
ctrl.ke2		= ke2;
ctrl.trq_lim	= trq_lim;
str = sprintf('./spring%d/ctrl%d.mat',springN,springN);
save(str,'ctrl');



fprintf('Running controller:\n');
fprintf('Command Frequency: %f\n',f);
%			controller	log_file	time_start	time_stop	[controller_parms]
cmd_str = sprintf('sudo ./bin/fca_interface_headless 4 %s.log %.6f %.6f %.6f %.6f %.6f %d %d %d %d %.6f > /dev/null', log_name, t_start, t_end, a, w, b, k1, k2, ke1, ke2, trq_lim);
% cmd_str = sprintf('sudo ./bin/fca_interface_headless 6 %s.log %.6f %.6f %.6f %.6f %.6f %d %d %d %d %.6f > /dev/null', log_name, t_start, t_end, sys.K, sys.B, sys.I, f, a, trq_lim);

fprintf('%s\n\n',cmd_str);

ret = unix(cmd_str);
if (ret ~= 0)
	fprintf('Controller fail!\n');
	return;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

fprintf('Compiling logs\n');

fprintf('Opening log:\t');
fprintf('%s.log\n',log_name);

log_str = sprintf('%s.log',log_name);
log = importdata(log_str);

motor_angle		= log.data(:,4);
leg_angle		= log.data(:,5);

motor_current	= log.data(:,8);


t = log.data(:,1);

mtr_trq = motor_current*A2T;

spring_pos = motor_angle-leg_angle + OFFSET;

spring_trq = K*spring_pos;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure;
set(h,'position',[ 100 100 400 400])
hold on
% str = sprintf('Spring %d: Torques',springN);
% title(str)

plot([t(1) t(end)], [a a], 'k--','linewidth', 1);
plot([t(1) t(end)], [-a -a], 'k--','linewidth', 1);
plot([t(1) t(end)], [b b], 'k--','linewidth', 1);

pt = plot(t,mtr_trq,'g','linewidth', 2);

pd = plot(t,(b+a*sin(w*(t-0.0))),'r--','linewidth',2);

% plot(t,I*(a*w*w*-sin(w*(t-0.003)))/K,'r--','linewidth',2);

pm = plot(t,spring_trq,'linewidth', 2);


legend([pd; pm; pt],'Desired', 'Measured', 'Motor Torque')
axis tight

xlabel(	'\textbf{Time} $\left( s \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Torque} $\left( N \cdot m \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.15 0.15 0.8 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/track%d.pdf', springN);
if (save_plots == 1)
	save2pdf(str);
end

